An environment where Global Positioning Satellite (GPS) technology is not operational is referred to here as a “GPS-denied” environment. In GPS-denied environments, navigation systems that do not rely on GPS typically must be used. Historically, such navigation systems make use of an inertial measurement unit (IMU).
Recently, however, there has been significant interest in developing navigation systems for GPS-denied environments that do not completely rely on an IMU. One such approach employs a three-dimensional (3D) light detection and ranging (LIDAR) sensor. A 3D LIDAR produces a 3D range image of the environment. Using the 3D range image, it is possible to extract planes and other geometric shapes (also referred to here as “features”) in the environment. These features, if unique, can then be used as landmarks to aid navigation. A standard method of navigation using such landmarks employs simultaneous localization and mapping (SLAM). SLAM is used to build up a map within an environment while at the same time keeping track of a current location for a vehicle or person. Like a stochastic Kalman filter, SLAM needs an estimate of the location of the extracted feature and an estimate of the uncertainty in the location of the extracted feature.
It is important to capture the uncertainty in the location of an extracted feature accurately due to the dependence of the performance of SLAM on the quality of the location measurement and uncertainty estimate. If the uncertainty is not properly estimated the Kalman filter loses its optimality property and the measurements are not given the proper gain.